#include <string.h>
#include "usart.h"
#include "oslib.h"
#include "comm.h"

extern osMessageQueueId_t UART3_RxSema;
extern osMessageQueueId_t dispatchQueueHandle;

void RecvTask(void *argument) 
{
    static wobot_down_message *dmsg_ptr;
    // static wobot_down_message dmsg;
    static wobot_down_message_payload dmpl;
    for (;;) 
    {
        osSemaphoreAcquire(UART3_RxSema, osWaitForever);
        dmsg_ptr = (wobot_down_message *)UART3_TaskBuffer;
        if (dmsg_ptr->pre != WOBOT_PRE_LE) {
            uprintf("[Warn] RecvTask: PRE = %x\n", dmsg_ptr->pre);
            continue;
        }
        if (dmsg_ptr->len < 3 || dmsg_ptr->len > 13) {
            uprintf("[Warn] RecvTask: LEN = %d\n", dmsg_ptr->len);
            continue;
        }
        if (dmsg_ptr->idx != WOBOT_IDX) {
            uprintf("[Warn] RecvTask: IDX = %x\n", dmsg_ptr->idx);
            continue;
        }
        if (dmsg_ptr->action != GET && dmsg_ptr->action != RUN) {
            uprintf("[Warn] RecvTask: ACTION = %d\n", dmsg_ptr->action);
            continue;
        }
        if ((dmsg_ptr->param)[dmsg_ptr->len - 3] != WOBOT_A) {
            uprintf("[Warn] RecvTask: A = %x\n", (dmsg_ptr->param)[dmsg_ptr->len - 3]);
            continue;
        }
        dmpl.action = dmsg_ptr->action;
        dmpl.device_id = dmsg_ptr->device_id;
        dmpl.param_len = dmsg_ptr->len - 3;
        memcpy(dmpl.param, dmsg_ptr->param, dmpl.param_len);
        if (osOK != osMessageQueuePut(dispatchQueueHandle, &dmpl, NULL, 1000)) {
            uprintf("[Error] RecvTask: Failed to put message\n");
        }
    }
}
